#include "move_plan_sys.h"
#include <stdio.h>
#include <unistd.h>
//#include <ctime>
#include <sys/times.h>

#include <signal.h>
#include <opencv2/opencv.hpp>

#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PointStamped.h>
 
using namespace std;
using namespace cv;
bool rosShutDown = false;
void mySiginHandler(int sig){

    rosShutDown = true;
}

int main(int argc,char** argv)
{  



    ros::init(argc, argv, "move_drive_plan");  
    ros::NodeHandle n;
    signal(SIGINT, mySiginHandler);
#if 1
    ljl::TRACKPATH TrackPath;
    ljl::BOOSTPATH BoostPath;

    if(argc != 2){
	cout << "argc is erros" << endl;
        exit(-1);
    }
        ljl::MOVE_PLAN_SYS* Move_Plan_Sys = new ljl::MOVE_PLAN_SYS(&TrackPath, &BoostPath, argv[1]);
	Move_Plan_Sys -> loadGoalPose();
	cout << Move_Plan_Sys->mvTrackPaths.size() << "    ******"<<endl;
	//Move_Plan_Sys -> mvTrackPaths;	//std::vector<std::vector<float> >

    ros::Subscriber sub = n.subscribe("/vision_pose360",1, &ljl::MOVE_PLAN_SYS::poseCallback, Move_Plan_Sys);
    ros::Publisher pub_cmd = n.advertise<geometry_msgs::Twist>("/control_velocity_nav", 1);

    ros::Rate loop_rate(20);	//设置发送数据的频率为10Hz

    geometry_msgs::Twist cmd;
    geometry_msgs::PointStamped goal_point_stamped;
    int track_size = Move_Plan_Sys-> mvTrackPaths.size();

	float vT = 0;	// record last v of robot
        float wT = 0;	//record last w of robot
	cmd.linear.x = 1;
        cmd.angular.z = 0;


#if 0
    clock_t startT, endT;
    startT = times(NULL);

    while (1){

	if(rosShutDown) break;
	
	cmd.linear.x = 1;
        cmd.angular.z = 0;
	
        Move_Plan_Sys->slow_start(cmd, vT, wT);
	vT = cmd.linear.x;
	wT = cmd.angular.z;

	Move_Plan_Sys->mstatus_v = cmd.linear.x;
	Move_Plan_Sys->mstatus_w = cmd.angular.z;
	pub_cmd.publish(cmd);	//向话题发布消息
	ros::spinOnce();	
	loop_rate.sleep();	//按前面设置的10Hz频率将程序挂起

    }
    endT = times(NULL);
    //double con = CLOCKS_PER_SEC;  //100
    double tt = double(endT - startT)/sysconf(_SC_CLK_TCK);  //1000 000
    cout << "run time is:  " << tt << " s"<<endl;
    if (tt != 0 ){
    	cout << "speed is:  " << 90/( tt ) << "  /s"<<endl;
    }
#endif

#if 1
    for(int i=0; i<track_size; i++){
        if(rosShutDown) break;
	const float x_goal = Move_Plan_Sys->mvTrackPaths[i][0];
	const float y_goal = Move_Plan_Sys->mvTrackPaths[i][2];
	const float theta_goal = Move_Plan_Sys->mvTrackPaths[i][3];

        ros::spinOnce();

	//if not callback vo data, then wait
	if(!Move_Plan_Sys->miscallback){
		i--;
		usleep(1000);
		cout << "node :: wait ..." <<endl;
		continue;
	}
	Move_Plan_Sys->miscallback = false;
	
        float robot_v = 0.0;
        float robot_angular = 0.0; 

	//compute the speed v and angular of robot
        bool bok = Move_Plan_Sys->xycovert2vw(  Move_Plan_Sys->mrobot_x,
						Move_Plan_Sys->mrobot_y,
						Move_Plan_Sys->mrobot_w,
						x_goal,
						y_goal,
						theta_goal,
						robot_v,
						robot_angular
						);
	if(!bok){
		cmd.linear.x = robot_v;
        	cmd.angular.z = robot_angular;

	//>>>	check v for up speed slowly
		Move_Plan_Sys->slow_start(cmd, vT, wT);
		vT = cmd.linear.x;
		wT = cmd.angular.z;
		Move_Plan_Sys->mstatus_v = cmd.linear.x;
		Move_Plan_Sys->mstatus_w = cmd.angular.z;
	//<<<

        	pub_cmd.publish(cmd);	//向话题发布消息
		cout << "move_drive_plan_node.cc :: linear.x =  "<<robot_v<<"  angular.z =  "<< robot_angular << endl;
		cout << "move_drive_plan_node.cc ::  **" <<endl;
		i--;
        	loop_rate.sleep();	//按前面设置的10Hz频率将程序挂起	
        }
	else{
		//break;
	    cout << "node :: " << "nest point >>>>>>>>>>>>>>>>>>>>>"<<endl;
	}
       
    }
#endif

    Move_Plan_Sys->fast_stop(pub_cmd, cmd);
    //Move_Plan_Sys->slow_stop(pub_cmd, cmd);

    delete Move_Plan_Sys;
    ros::shutdown();
    cout << "stop move  ****************"<<endl;
#endif
 return 0;  
}

#if 0
Twist - 线速度角速度
geometry_msgs/Twist

geometry_msgs/Vector3 linear
    float64 x
    float64 y
    float64 z
geometry_msgs/Vector3 angular
    float64 x
    float64 y
    float64 z
#endif

